#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <list>
#include <map>

#include "Map.h"
#include "Point.h"
#include "Node.h"

using namespace std;

#define FACTOR 10

bool FindPath();
list<Node*> GetNeighbors(Node* node);
int GetHeuristic(Node* node);
Node* NodeWithLowestScore();
void AddNeighborNode(int diffX, int diffY, Node* parent, list<Node*> *n);
int GetDifference(Node* n, Node* p);
bool InSet(Node*, list<Node*> set);

list<Node *> lopen;
list<Node *> lclosed;
Map astarMap;
map<char, float> gValues;

int main(int argc, char* argv[])
{
  if (argc != 2) {
    cout << "Usage: " << argv[0] << " mapFile" << endl;
    exit(1);
  }

  gValues['-'] = 1.0;
  gValues['M'] = 1.3;
  gValues['S'] = 1.0;
  gValues['E'] = 1.0;

  if (!astarMap.LoadMap(argv[1])) {
    cout << "Unable to load map: " << argv[1] << endl;
    exit(2);
  }
  astarMap.PrettyPrint();

  if (FindPath()) {
    cout << "Path Found!" << endl;
    astarMap.PrintPath();
  } else {
    cout << "No Path Exists!" << endl;
  }

  return 0;
}

bool FindPath()
{
  int tentativeG;
  bool tentativeIsBetter;
  list<Node*> neighbors;
  list<Node*>::iterator nit;

  Node* curNode = astarMap.GetBoardNode(astarMap.GetStartPoint());
  lopen.push_back(curNode);

  curNode->SetG(0);
  curNode->SetH(GetHeuristic(curNode));
  curNode->SetF(curNode->GetH());

  while (!lopen.empty()) {
    curNode = NodeWithLowestScore();


    lopen.remove(curNode);
    lclosed.push_back(curNode);

    if ((curNode->GetCoords()) == (astarMap.GetEndPoint())) {
      return true;
    }
    
    neighbors = GetNeighbors(curNode);

    //cout << *curNode << " | Num neighbors: " << neighbors.size() << endl;
    for (nit = neighbors.begin();
	 nit != neighbors.end();
	 ++nit) {
      if (InSet(*nit, lclosed)){
	continue;
      }

      tentativeG = (**nit).GetTentativeParent()->GetG() + 
	GetDifference(*nit, (**nit).GetTentativeParent());

      if (!InSet(*nit, lopen)) {
	lopen.push_back(*nit);
	tentativeIsBetter = true;
      } else if (tentativeG < (**nit).GetG()) {
	tentativeIsBetter = true;
      } else {
	tentativeIsBetter = false;
      }

      if (tentativeIsBetter) {
	(**nit).SetParent(curNode);

	(**nit).SetG(tentativeG);
	(**nit).SetH(GetHeuristic(*nit));
	(**nit).SetF((**nit).GetG() + (**nit).GetH());

	//cout << "\tSetting: " << **nit << " | Parent: " << *curNode << endl;
      }
    }
  }

  return false;
}

list<Node*> GetNeighbors(Node* node)
{
  list<Node*> n;

  // Note: (0, 0) is in the top left corner
  AddNeighborNode( 0, -1, node, &n); // North
  AddNeighborNode( 1, -1, node, &n); // Northeast
  AddNeighborNode( 1,  0, node, &n); // East
  AddNeighborNode( 1,  1, node, &n); // Southeast
  AddNeighborNode( 0,  1, node, &n); // South
  AddNeighborNode(-1,  1, node, &n); // Southwest
  AddNeighborNode(-1,  0, node, &n); // West
  AddNeighborNode(-1, -1, node, &n); // Northwest

  return n;
}

void AddNeighborNode(int diffX, int diffY, Node* parent, list<Node*> *n)
{
  Point p  = parent->GetCoords();
  Node* boardNode;

  //cout << "Coords: (" << p.GetX() << ", " << p.GetY() << ") - "
  //     << " x: " << diffX << " | y: " << diffY << endl;

  if (astarMap.IsWalkable(p.GetX() + diffX, p.GetY() + diffY)) {
    if ((boardNode = astarMap.GetBoardNode(p.GetX() + diffX,
					   p.GetY() + diffY)) != NULL) {
      //cout << "Adding " << *boardNode
      //   << " | Parent: " << *parent << endl;
      boardNode->SetTentativeParent(parent);
      n->push_back(boardNode);
    }
  }
}

Node* NodeWithLowestScore()
{
  Node* n = lopen.front();
  for (list<Node*>::iterator lit = lopen.begin(); lit != lopen.end(); ++lit) {
    if ((**lit).GetF() < n->GetF()) {
      n = *lit;
    }
  }

  return n;
}

int GetDifference(Node* n, Node* p)
{
  Point p1 = n->GetCoords();
  Point p2 = p->GetCoords();

  double dist = (FACTOR * sqrt(pow((double)(p1.GetX() - p2.GetX()), (double)2) + 
			      pow((double)(p1.GetY() - p2.GetY()), (double)2)));  

  return (int)(gValues.find(n->GetData())->second * dist);
}

int GetHeuristic(Node* node)
{
  Point np = node->GetCoords();
  Point ep = astarMap.GetEndPoint();

  
  return (int)(FACTOR * sqrt(pow((double)(ep.GetX() - np.GetX()), (double)2) +
			 pow((double)ep.GetY() - np.GetY(), (double)2)));
}

bool InSet(Node* node, list<Node*> set)
{
  if (find(set.begin(), set.end(), node) != set.end())
    return true;
  return false;
}
